#!/usr/bin/env python
# coding: utf-8

# In[1]:


import numpy as np
import math as m


# In[2]:


#直线轨迹插补（首尾以抛物线过渡）
#输入参数：起始点位姿S=[x1,y1,z1,alpha1,beta1,gama1]
#          目标点位姿D=[x2,y2,z2,alpha2,beta2 gama2]  
#          [x,y,z]为基坐标系下的三维坐标
#          [alpha,beta,gama]为绕x,y,z三轴的转角
#          机械臂末端速度vel，加速度accl，插补时间间隔t_0
#输出参数：直线轨迹上各插补点的齐次变换矩阵T
def spaceline(S,D,vel,accl,t_0): 
    x1,y1,z1,alpha1,beta1,gama1=S
    x2,y2,z2,alpha2,beta2,gama2=D
    delta_x,delta_y,delta_z,delta_alpha,delta_beta,delta_gama=D-S
    leng=np.sqrt(delta_x**2+delta_y**2+delta_z**2) #直线轨迹长度
    T1=vel/accl
    S1=0.5*accl*T1**2 #加减速段的时间和位移
    T = 2*T1 + (leng - 2*S1) / vel #总时间
    N=m.ceil(T/t_0)-1 #插补数
    S1_ = S1 / leng
    T1_ = T1 / T
    T2_ = 1 - T1_
    accl_ = 2*S1_ / T1_**2 #归一化处理
    lambda_= np.zeros((N,1))
    for i in range(N):
        t =(i+1)/N
        if t>=0 and t<=T1_:
            lambda_[i]=0.5*accl_*t**2
        elif t>T1_ and t<=T2_:
            lambda_[i]=0.5*accl_*T1_**2+accl_*T1_*(t-T1_)
        elif t>T2_ and t<=1:
            lambda_[i]=0.5*accl_*T1**2+accl_*T1_*(T2_-T1_)+0.5*accl_*(t-T2_)**2 #归一化因子lambda计算
    x=np.zeros(N)
    y=np.zeros(N)
    z=np.zeros(N)
    alpha=np.zeros(N)
    beta=np.zeros(N)
    gama=np.zeros(N)
    T=[0]*N
    for i in range(N):
        x[i]=x1+delta_x*lambda_[i]
        y[i]=y1+delta_y*lambda_[i]
        z[i]=z1+delta_z*lambda_[i]
        alpha[i]=alpha1+delta_alpha*lambda_[i]
        beta[i]=beta1+delta_beta*lambda_[i]
        gama[i]=gama1+delta_gama*lambda_[i]
        cal,sal=np.cos(alpha[i]),np.sin(alpha[i])
        cbe,sbe=np.cos(beta[i]),np.sin(beta[i])
        cga,sga=np.cos(gama[i]),np.sin(gama[i])
        T[i]=np.array([[cbe*cga, -cbe*sga, sbe, x[i]],
                      [cal*sga+cga*sal*sbe, cal*cga-sal*sbe*sga, -cbe*sal, y[i]],
                      [sal*sga-cal*cga*sbe, cga*sal-cal*sbe*sga, cal*cbe, z[i]],
                      [0, 0, 0, 1]]) 
    return T


# In[3]:


#测试代码
S=np.array([0,0,0,0,0,0])
D=np.array([0,4,3,0,0,0])
vel=1
accl=1
t_0=0.5
spaceline(S,D,vel,accl,t_0)

# In[ ]:




